/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <iostream>

#include <opencv/highgui.h>
#include <time.h>

#include "error/cerror.h"
#include "vision/cimagecamera.h"
#include "vision/cimagecamthread.h"
#include "vision/pointpyrmatch.h"
#include "vision/cpointmatch.h"

#include "katanavision/ckatanasensing.h"
#include "katanavision/katanafuncs.h"

using namespace std;

//1 : PATTERN FIND TEST
int main1( int argc, char *argv[] )
{	
	timespec ts1, ts2;
		
	try {
		IplImage * pattern = cvLoadImage("pattern.jpg");
		IplImage * test = cvLoadImage("test1.jpg");
				
		CPointPyrMatch detector(cv::Mat(pattern), cv::Mat(test), 0.8);
		//CPointMatch detector(pattern);
		
		cvNamedWindow("win", 1);

		cv::Point2f i_point;
		clock_gettime(CLOCK_REALTIME, &ts1);
		if (detector.DetectPoint(test, i_point)) {
			clock_gettime(CLOCK_REALTIME, &ts2);
			std::cout << i_point.x << " " << i_point.y << std::endl;
			cvCircle(test, i_point, 3, cvScalar(0,0,255), -1);
			std::cout << "time = " << double(ts2.tv_nsec - ts1.tv_nsec)/1.0e6 + double(ts2.tv_sec - ts1.tv_sec)*1000 << " ms" << endl;
			cvShowImage("win", test);
			cvWaitKey(0);
		}
		cvReleaseImage(&test);
		cvReleaseImage(&pattern);
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
	return 0;
}

//2 : KATANAFUNC TEST
int main( int argc, char *argv[] )
{	
	try {
		
		// create katana
		TCdlCOMDesc ccd = {4, 57600, 8, 'N', 1, 300, 0};
		CCdlCOM device(ccd);
		CCplSerialCRC protocol;
		protocol.init(&device);
		Katana::CKatanaSensing katana;
		katana.create ( "katana5M180.cfg", &protocol );
		
		katana.unBlock();
		
		katana.moveRobotTo(400, 0, 100, 0, 0, 0, true);
		/*katana.UpdateSensors();
		katana.movDegrees(M_GRP, 70, true);
		katana.CalculateNoObstacleValues(10);
		katana.movDegrees(M_GRP, 5, true);
		katana.UpdateSensors();*/

		Katana::PseudoLinearMove(katana, Point3d(400, 0, -100), 30, 20);
	}
	catch ( Exception &e ) {
		std::cout << "ERROR: " << e.message() << std::endl;
	}
	catch ( Errors::CError& e ) {
		e.Report(std::cout);
	}
	return 0;
}

